/* Author: Binh Cao */

#include "DemoSensor.h"
namespace fb = flyingcamera::base;



void flyingcamera::base::moving::DemoSensor::getNearObstacles(CameraStateValidityCheckerM* checker) const
{

	checker->setStartTime(time_);
	checker->removeMObstacles();

	for(unsigned i=0; i<m_obstacles.size();i++){
		//fcl::CollisionGeometry* const obstacle = m_obstacles[i];
		fcl::Box* const obstacle = static_cast<fcl::Box*>(m_obstacles[i]);
		fb::VehiclePath* path = m_obstacles_path[i];


		double x, y, vx, vy;
		path->getState(time_, x, y, vx, vy);

		fcl::Vec3f v(x, y, obstacle->side[2]/2);
		//fcl::Vec3f v(x+tf.getTranslation()[0], y+tf.getTranslation()[1], tf.getTranslation()[2]);
		const fcl::Transform3f of(v);
		const std::pair< double, double > velocity(vx, vy);
		checker->addMObstacle(obstacle, of, velocity);
	}
}

void flyingcamera::base::moving::DemoSensor::addMObstacle(fcl::CollisionGeometry* const obstacle, flyingcamera::base::VehiclePath* const m_path)
{
	m_obstacles.push_back(obstacle);

	m_obstacles_path.push_back(m_path);
}

void flyingcamera::base::moving::DemoSensor::setTime(double time)
{
	time_ = time;
}
